#include <iostream>
#include <cstdlib>

#include "stegutils.hpp"

namespace StegUtils
{

//@param: long pByecount: total bytes from data to grab bits from
//@param: int pDepth: bit depth, total bits per byte to grab
//@param: vuc_t& pOutput, empty vector of unsigned char
vuc_t::const_iterator decode(int pDepth, unsigned long pBytecount, vuc_t::const_iterator& pDataiter, const vuc_t& pData, vuc_t& pOutput)
{

    if(pBytecount > pData.size())
    {
        std::cout<<"Decoding ERROR - not enough data to decode byte count of "<<pBytecount<<std::endl;
        exit(1);
    }
    if(8 < pDepth)
    {
        std::cout<<"Decoding ERROR - bit depth too high, MAX - 8"<<std::endl;
        exit(1);
    }
    if(0 >= pDepth)
    {
        std::cout<<"Decoding ERROR - bit depth too low, MIN - 1"<<std::endl;
        exit(1);
    }

    pOutput.clear();

    ui_t datashift = 128;

    ui_t totalbits = pBytecount*8;
    uc_t databyte = 0;
    ui_t bit = 0;
    while(bit<totalbits)
    {
        for(ui_t depth= pDepth; depth>0; --depth)
        {
            uc_t depthshift = 1;
            depthshift = depthshift<<(depth-1);

            uc_t databit = *pDataiter&depthshift;

            if(databit>0)
            {
                databyte |= datashift;
            }
            else if (0 == databit)
            {
                //ignore
            }

            datashift = datashift>>1;

            if(0 == datashift)
            {
                pOutput.push_back(databyte);
                datashift = 128;
                databyte = 0;
            }
            ++bit;
        }
        ++pDataiter;
        if(pDataiter==pData.end())
        {
            return pData.end();
        }
    }
    return --pDataiter;
}

vuc_t::iterator encode(int pDepth, const vuc_t& pData, vuc_t::iterator& pOutputiter, vuc_t& pOutput)
{

    if(8 < pDepth)
    {
        std::cout<<"Encoding ERROR - bit depth too high, MAX - 8"<<std::endl;
        exit(1);
    }

    if(0 >= pDepth)
    {
        std::cout<<"Encoding ERROR - bit depth too low, MIN - 1"<<std::endl;
        exit(1);
    }

    vuc_t::const_iterator dataiter;

    ui_t datasize = pData.size();

    ui_t requiredsize = datasize * 8 / pDepth;

    if(pOutput.size() >= requiredsize)
    {
        dataiter = pData.begin();

        ui_t totalbits = datasize*8;
        ui_t datashift = 128;

        for(ui_t bit=0; bit<totalbits; ++bit)
        {

            for(ui_t outputdepth=pDepth; outputdepth>0; --outputdepth)
            {
                uc_t databit = *dataiter;
                databit &= datashift;

                uc_t depthshift = 1;
                depthshift=depthshift<<(outputdepth-1);

                if(databit > 0)
                {
                    *pOutputiter = *pOutputiter | depthshift;
                }
                else if (databit == 0)
                {
                    uc_t zeroshift = ~depthshift;
                    *pOutputiter = *pOutputiter & zeroshift;
                }

                datashift = datashift>>1;

                if(datashift == 0)
                {
                    ++dataiter;
                    datashift = 128;
                    if(dataiter==pData.end())
                    {
                        return pOutputiter;
                    }
                }
            }
            ++pOutputiter;
            if(pOutputiter==pOutput.end())
            {
                return pOutput.end();
            }
        }

    }
    else
    {
        std::cout<<"ERROR: Output not large enough for bit depth of: "<<pDepth<<std::endl;
        exit(1);
    }
    return pOutput.end();
}

vuc_t encodeIntToCharArray(ui_t integer)
{

    int i;
    ui_t t,st;
    vuc_t d(4);

    st = integer;
    t = integer;

    for(i=0; i<4; i++)
        d[i]=0;

    if(st < 256)
    {
        d[0] = 0;
        d[1] = 0;
        d[2] = 0;
        d[3] = integer;
        return d;
    }
    for(i=0; i<4; i++)
    {
        int c = 0;

        while(st>=256)
        {
            t = t/256;
            if(t < 256)
            {
                d[i] = t;
                for(int j=0; j<=c; j++)
                    t *=256;
                t = st - t;
                st = t;
                break;
            }
            c++;
        }
        if(st < 256 && i < 3)
        {
            d[3] = st;
            if(d[0] != 0)
            {
                if(d[1] != 0)
                {
                    if(d[2] != 0)
                        continue;
                    d[2] = d[1];
                    d[1] = d[0];
                    d[0] = 0;
                }
                else if(d[1]==0)
                {
                    d[2] = d[0];
                    d[0] = 0;
                }
                break;
            }
        }
    }
    return d;
}

//Left to right for most significant value
ui_t decodeIntFromCharArray(const vuc_t& pCharArray)
{
    if( 0 == pCharArray.size() )
    {
        return 0;
    }

    ui_t integer = 0, base = 1;

    vuc_t::const_reverse_iterator chariter;
    for(chariter=pCharArray.rbegin(); chariter != pCharArray.rend(); ++chariter)
    {
        integer += (ui_t)*chariter*base;
        base *= 256;
    }

    return integer;

}

ui_t getMaxStegObjectSize(int pDepth, const vuc_t& pData)
{
    if(pData.size() > 4)
    {
        return ( pDepth * ( pData.size() - 4 ) / 8 );
    }

    return 0;

}

}
